#include <stdio.h>
#include <boost/bind.hpp>
#include <boost/thread/xtime.hpp>
#include "serial_protocol.h"

SerialProtocol *SerialProtocol::createInstance(char const*p_sDevice, long p_lBaud, void_function p_pFuncIncomingEvent, void_function p_pFuncStatusChanged)
{
  SerialProtocol *pSerial=new SerialProtocol(p_lBaud);
  if (!pSerial->setDevice(p_sDevice))
  {
    delete pSerial;
    return NULL;
  }
  pSerial->m_funcIncoming=p_pFuncIncomingEvent;
  pSerial->m_funcStatus=p_pFuncStatusChanged;
  return pSerial;
}

SerialProtocol::SerialProtocol(long p_lBaud) :
    SerialCommunication::SerialCommunication(p_lBaud),
    m_ptControl(NULL),
    m_bAllowZeroInData(true),
    m_bConnected(false),
    m_bControlWantRun(false)
{
}

SerialProtocol::~SerialProtocol()
{
  disconnect();
}

void SerialProtocol::handleReceivedBytes()
{
  unsigned char t;
  if (getBytesAvailable()<1)
    return; // should not happen at all!

  m_condIncoming.notify_all();
  switch(m_State.getState())
  {
  case ControlState::eState_Init:
    break;
  case ControlState::eState_Connect:
    if (getBytesAvailable()<2)
      return;
    while (getBytesAvailable()>0)
    {
      getByte(&t);
      if (t==0)
      {
        // it was a "0"-byte, now clean all others from queue
        while (peekByte(&t)>0 && t==0)
        {
          // classic init/reset-sequence
          getByte(&t);
        } // if not 0 then possibly already data?
        m_bConnected=true;
        m_State.setState(ControlState::eState_Wait4Header);
        m_dMessage.clear();
        m_cConnectionStateChanged.notify_all();
        m_funcStatus();
        break;
      }
    }
    break;
  case ControlState::eState_Wait4Header:
    if (getBytesAvailable()<2)
      return;
    while(2 > m_dMessage.size() && getBytesAvailable()>0)
    {
      peekByte(&t);
      if (t==0)
      {
        // reset/init during connection?
        m_bConnected=false;
        m_cConnectionStateChanged.notify_all();
        m_funcStatus();

        handleReceivedBytes();
        return;
      }
      getByte(&t);
      m_dMessage.push_back(t);
    }
    if (m_dMessage.size()>=2)
      parseHeader();
    break;
  case ControlState::eState_Wait4Data:
    while(m_iWaiting4Bytes>m_dMessage.size() && getBytesAvailable()>0)
    {
      peekByte(&t);
      if (t==0 && !m_bAllowZeroInData)
      {
        m_State.setState(ControlState::eState_Reset);
        return;
      }
      getByte(&t);
      m_dMessage.push_back(t);
    }
    if (m_iWaiting4Bytes<=m_dMessage.size())
    {
      parseMessage();
    }
    break;
  case ControlState::eState_Disconnect:
  case ControlState::eState_Reset:
  default:
    // reset/init during connection?
    m_bConnected=false;
    m_iWaiting4Bytes=0;
    m_State.setState(ControlState::eState_Wait4Header);
    m_dMessage.clear();
    m_cConnectionStateChanged.notify_all();
    m_funcStatus();

    handleReceivedBytes();
    break;
  }
}

void SerialProtocol::parseHeader()
{
  // should NOT happen!
  if (m_dMessage.size()<2)
    return;

  unsigned char h1=m_dMessage.front();
  m_dMessage.pop_front();
  unsigned char h2=m_dMessage.front();
  m_dMessage.push_front(h1);

  if (h2&0x04)
    m_iWaiting4Bytes=2 + (h2&3) + 1; // attached value (1 to 4 bytes)
  else if (h2&0x02)
    parseMessage(); // direct value
  else
    m_iWaiting4Bytes=3; // next byte will reveal true size
}

void SerialProtocol::parseMessage()
{
  unsigned char major, minor, size, command, value=0xFF;
  unsigned char h1, h2;
  bool bPushSize;

  if (m_dMessage.size()<2)
    return;

  h1=m_dMessage.front();
  m_dMessage.pop_front();
  h2=m_dMessage.front();
  m_dMessage.pop_front();

  major=h1 >> 2;
  size=0;
  if (h2&0x04)
  {
    // attached value (1 to 4 bytes)
    size=(h2&3) + 1;
  } else if (h2&0x02)
  {
    size=0; // direct value
  } else
  {
    if (m_dMessage.size()>0)
    {
      size=m_dMessage.front(); // dynamic
      bPushSize=true;
    } else
      size=1;
  }

  // error - not enough data?!?
  if (m_dMessage.size() < size)
  {
    // restore data-stack
    if (bPushSize)
      m_dMessage.push_front(size);
    m_dMessage.push_front(h2);
    m_dMessage.push_front(h1);
    m_iWaiting4Bytes=size+2;
    if (bPushSize)
      ++m_iWaiting4Bytes;
    return;
  }

  // correct size, all data collected
  minor= h2 >> 3;
  command=h1&3;
  if (size==0)
    value=h2&0x01;
  else
  {
    // TODO combined values, get them from stack
  }
  m_iWaiting4Bytes=0;
  m_dMessage.clear();
  m_State.setState(ControlState::eState_Wait4Header);
  if (major==0 && command==1)
    fprintf(stderr, "ping from HW-Master\n");
  else
    fprintf(stderr,"maj,min: %i %i -> cmd=%02X -> value=%i\n", major, minor, command, value);
  // TODO - here we should actually handle the incoming event
  m_funcIncoming();
}

void SerialProtocol::handleError(int p_iID, char const *p_sError)
{
}

int SerialProtocol::connect(bool p_bBlocking)
{
  int result=-1;

  if (m_bConnected)
    return 1;

  result=openConnection();
  if (m_ptControl!=NULL)
  {
    m_ptControl->join();
    delete m_ptControl;
  }
  m_bControlWantRun=true;
#ifdef LINUX
  m_ptControl=new boost::thread(boost::bind(&SerialProtocol::controlThread, this));
#endif
  if (!p_bBlocking || result<1)
    return result;

  scoped_lock lock(m_syncWait);
  while(!m_bConnected)
  {
    m_cConnectionStateChanged.wait(lock);
  }
  return 1;
}

void SerialProtocol::disconnect()
{
  closeConnection();
  m_bControlWantRun=false;
  if (m_ptControl)
  {
    m_condIncoming.notify_all();
    m_ptControl->join();
    fprintf(stderr, "control-thread joined\n");
    delete m_ptControl;
  }
}

void SerialProtocol::controlThread()
{
  int count=0;
  bool bWasTimeout=false;
  boost::xtime now;

  // loop endless
  while (m_bControlWantRun)
  {
    switch (m_State.getState())
    {
    case ControlState::eState_Init:
      m_State.setState(ControlState::eState_Connect);
      // FALL THRUE!
      //break;
    case ControlState::eState_Connect:
      sendByte(0);
      sendByte(0);
      fprintf(stderr, ".");
      break;
    case ControlState::eState_Reset:
      m_bConnected=false;
      m_dMessage.clear();
      m_State.setState(ControlState::eState_Connect);
      break;
    default:
      // send a ping?
      if (bWasTimeout)
      {
        if (count>3)
        {
          m_bConnected=false;
          m_State.setState(ControlState::eState_Reset);
          m_cConnectionStateChanged.notify_all();
          m_funcStatus();
          count=0;
          fprintf(stderr,"SERCOM: disconnected, no response from hardware\n");
        } else
        {
          sendByte(1);
          sendByte(2);
          ++count;
        }
      }
      break;
    }

    // go to sleep - at least for a certain time
    bWasTimeout=false;
    if (m_State.getState()==ControlState::eState_Reset)
      continue;
    if (boost::xtime_get(&now, boost::TIME_UTC)!=0)
    {
      boost::recursive_mutex::scoped_lock lock(m_syncWait);
      now.sec+=1;
      if (!m_condIncoming.timed_wait(lock, now))
      {
        // deal with a timeout
        bWasTimeout=true;
      } else
      {
        count=0;
      }
    }
  }
  //fprintf(stderr, "I do not want anymore\n");
}
